/*
===========================================================================
This source file is part of DyLab (Dynamic Laboratory)
For the latest info, see http://dylab-modules.googlecode.com

Copyright (c) 2006-2008 Lukas Krejci
(krejci.lukas@volny.cz)

This file is part of DyLab.

    DyLab is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    DyLab is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with DyLab.  If not, see <http://www.gnu.org/licenses/>.
===========================================================================
*/

//===========================================================================================
#include "T4HSpace_CollisionHandler.h"
#include <dylab/utils/geometry/AxisAlignedBox.h>
#include <dylab/simulation/Simulation.h>

//===========================================================================================
const dylab::String dylab::T4HSpace_CollisionHandler::COMPONENT_NAME = "t4hspace";

//===========================================================================================
#define OP_PENETRATION_FORCES_NAME          "penetrationForces"
#define OP_PENETRATION_FORCES_MAXOBJECTS    32

//===========================================================================================
dylab::T4HSpace_CollisionHandler::T4HSpace_CollisionHandler() 
: ComCollisionHandler(COMPONENT_NAME), mGridCellSize(8), mDebugCollisionsEnabled(false)
{
}
//-----------------------------------------------------------------------------
dylab::T4HSpace_CollisionHandler::~T4HSpace_CollisionHandler() 
{
        // delete our object pool controller
        delete mObjPoolController;

        ComGfx::IScene::ScopedExLock(mParentSimulation->getGfx())
            ->destroyObjectPool(OP_PENETRATION_FORCES_NAME);
}
//-----------------------------------------------------------------------------
dylab::String dylab::T4HSpace_CollisionHandler::getDescription() const
{    
    // return description String
    return String::fmt("%1% (cell size=%2%)", getFullName(), mGridCellSize);
}
//-----------------------------------------------------------------------------
void dylab::T4HSpace_CollisionHandler::initialize(Simulation * _parentSimulation)
{
    // check & store simulation pointer
    DYLAB_ASSERT(_parentSimulation != NULL);
    mParentSimulation = _parentSimulation;

    // zero hash table entries
    for (uint32_t i = 0; i < HASH_TABLE_SIZE; i++)
        mMassPointHashTable[i] = NULL;

    // reset the iteration counter
    mIterationCounter = 0;

        // create object pool for debugging penetration forces
        ComGfx::IScene::ScopedExLock(mParentSimulation->getGfx())
            ->createObjectPool(OP_PENETRATION_FORCES_NAME, OP_PENETRATION_FORCES_MAXOBJECTS);

        // create & register our object pool controller as rendering event listener
        mObjPoolController = new OneShotObjectPoolController::Instance(new OneShotObjectPoolController(OP_PENETRATION_FORCES_NAME));
        OneShotObjectPoolController::ScopedExLock(mObjPoolController)->initialize(mParentSimulation);
        mParentSimulation->getGfxEventDispatcher()->registerListener(mObjPoolController);
}
//-----------------------------------------------------------------------------
void dylab::T4HSpace_CollisionHandler::loadFromPropertyTree(const PropertyTree & _pt)
{
    // apply the loaded properties
    setGridCellSize(_pt.getNodeValueT("grid_cell_size", getGridCellSize()));

    const PropertyTree::Node * debugCollisions = _pt.findNode("debugCollisions");
    if (debugCollisions != NULL)
    {
        setDebugCollisionsEnabled(debugCollisions->getValue().parse<bool>());
        setDebugCollisionsMaterial(debugCollisions->getChildNodeValue("material"));
    }
}
//-----------------------------------------------------------------------------
dylab::Simulation * dylab::T4HSpace_CollisionHandler::getParentSimulation()
{
    return mParentSimulation;
}
//-----------------------------------------------------------------------------
const dylab::Simulation * dylab::T4HSpace_CollisionHandler::getParentSimulation() const
{
    return mParentSimulation;
}
//-----------------------------------------------------------------------------
void dylab::T4HSpace_CollisionHandler::handleCollisions()
{ 
    //DYLAB_TRACE_CALLS;

    // creal collisions from previous iteration
    mCollisions.removeAll();

    // first, pre-retrieve (lock) modelInfo pointers for all simulation objects (for multithreaded collision detection consistency)
    // so we select all simulation objects, that has models supporting tetrahedral volume interface, and lock their models
    mProcessingModels.removeAll();
    for (Simulation::SimObjects::Iterator simObject(mParentSimulation->simObjectIterator()); simObject.isValid(); simObject++)
    {
        ComModelStructure * modelInfo = simObject->getModelStructure()->exLock();
        // we will process only objects, that has models supporting tetrahedral volume interface
        if (modelInfo->isTetrahedralVolumeInterfaceSupported())
            mProcessingModels.insertAtEnd(ProcessingModelInfo(modelInfo));
        else        
            simObject->getModelStructure()->exUnlock();        
    }

    // second: walk through all simulation objects and setup the hash table with their mass-points
    for (ProcessingModels::Iterator modelInfo(mProcessingModels); modelInfo.isValid(); modelInfo++)
    {
        // retrieve ComModelStructure::IMassPoints of the current simulation object
        ComModelStructure::IMassPoints * iMassPoints = modelInfo->getModelStructure()->getMassPointsInterface();

        // walk through all mass points
        for (ComModelStructure::IMassPoints::MassPointMap::Iterator mp(iMassPoints->massPointIterator()); mp.isValid(); mp++)
        {
            // compute discretized coords of the mass point position
            int32_t i, j, k;
            computeGridCoords(mp->currState().position, i, j, k);

            // hash the discretized coords
            uint32_t hash = hashGridCoords(i, j, k);            

            // log info
            //DYLAB_LOG5("\tMass point %1% has disrcetized coords (%2%, %3%, %4%) hashed to %5%", mp->currState().position, i, j, k, hash);

            // initialize or re-initialise current hash table entry, if needed
            MassPointHashTableEntry * mpHTEntry = mMassPointHashTable[hash];
            if (mpHTEntry == NULL)
            {
                mpHTEntry = new MassPointHashTableEntry(mIterationCounter);
                DYLAB_ASSERT(mpHTEntry != NULL);
                mMassPointHashTable[hash] = mpHTEntry;
                //DYLAB_LOGF("\t ---> new hash table entry created (mIterationCounter=%1%)", mIterationCounter);
            }
            else if (mpHTEntry->getTimeStamp() != mIterationCounter)
            {
                uint32_t n = mpHTEntry->infoList().getCount();
                //DYLAB_LOGF3("\t ---> hash table entry re-initialised (n=%1%, old=%2%, mIterationCounter=%3%)", n, mpHTEntry->getTimeStamp(), mIterationCounter);
                mpHTEntry->reset(mIterationCounter);                
            }

            // insert mass point info object into the entry
            MassPointInfo mpInfo(mp, modelInfo->getModelStructure());
            mpHTEntry->infoList().insertAtEnd(mpInfo);
        }
    }

    // third: walk trough all simulation objects and test their tetrahedrons
    for (ProcessingModels::Iterator modelInfo(mProcessingModels); modelInfo.isValid(); modelInfo++)
    {
        // retrieve ComModelStructure::ITetrahedralVolume pointer of the current simulation object
        ComModelStructure::ITetrahedralVolume * iTetraVol = modelInfo->getModelStructure()->getTetrahedralVolumeInterface();

        // walk through all tetrahedrons
        for (ComModelStructure::ITetrahedralVolume::TetrahedronMap::Iterator tetra(iTetraVol->tetrahedronIterator()); tetra.isValid(); tetra++)
        {
            // compute tetrahedron AABB
            TetrahedronGeometry tetraGeom = tetra->getGeometry();
            AxisAlignedBox tetraAABB;
            for (TetrahedronGeometry::VertexIdxIterator vertexIdx; vertexIdx.isValid(); vertexIdx++)
                tetraAABB.merge(tetraGeom.vertex(vertexIdx));

            // compute discretized coords of the tetrahedron AABB's minimum and maximum points
            int32_t iMin, jMin, kMin;
            computeGridCoords(tetraAABB.getMinPoint(), iMin, jMin, kMin);
            int32_t iMax, jMax, kMax;
            computeGridCoords(tetraAABB.getMaxPoint(), iMax, jMax, kMax);

            // walk through all grid cells occudied by the tetrahedron's AABB
            for (int32_t i = iMin; i <= iMax; i++)
            {
                for (int32_t j = jMin; j <= jMax; j++)
                {
                    for (int32_t k = kMin; k <= kMax; k++)
                    {
                        // hash (i, j, k) grid coords
                        uint32_t hash = hashGridCoords(i, j, k);

                        // get current hash table entry and check if contains any mass points from current iteration
                        MassPointHashTableEntry * mpHTEntry = mMassPointHashTable[hash];
                        if ((mpHTEntry != NULL) && (mpHTEntry->getTimeStamp() == mIterationCounter))
                        {
                            // walk through all hash table entry mass points
                            for (MassPointInfoList::Iterator mpInfo(mpHTEntry->infoList()); mpInfo.isValid(); mpInfo++)
                            {
                                // store mass point pointer
                                ComModelStructure::IMassPoints::MassPoint * mp = mpInfo->getMassPoint();

                                // store mass point position
                                Vector3 mpPos = mp->currState().position;

                                // test, whether tha mass point is not part of the tetrahedron itself
                                if (!tetra->containsMassPointId(mp->getId()))
                                {
                                    // test, if mass point position is within the tetrahedron's AABB
                                    if (tetraAABB.testPointInside(mpPos, true))
                                    {                                        
                                        // test, if mass point is inside the tetrahedron
                                        TetrahedronGeometry::pointcls_t pointCls = tetraGeom.classifyPoint(mpPos);
                                        switch (pointCls.first)
                                        {
                                        case Math::POINTCLS_INNER:
                                            mCollisions.insertAtEnd(CollisionInfo(mp, tetra(), 
												modelInfo->getModelStructure()));
                                            break;
                                        case Math::POINTCLS_EDGE:
                                        case Math::POINTCLS_FACE:
                                            DYLAB_LOGF2("Point contact: %1% (%2%)", mpPos, pointCls);
                                        }
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }
    }

    // finally, process all collisions
    for (CollisionInfoList::Iterator collision(mCollisions); collision.isValid(); collision++)
    {
        // store the collision & previous points 
        Vector3 collPos(collision->getMassPoint()->currState().position);
        Vector3 prevPos(collision->getMassPoint()->historyState(1).position);

        // store the collision edge & tetrahedron geometry
        TetrahedronGeometry tetraGeom(collision->getTetrahedron()->getGeometry());
        EdgeGeometry3       collEdge(prevPos, collPos);

        DYLAB_LOGF3("Collision found: %1%, collEdge: %2% (iteration: %3%)", collPos, collEdge, mIterationCounter);                        

        // if the point is moving, we can use its trajectory to compute the contant point
        if (!collEdge.isDegenerated())
        {
            // walk through all tetrahedron faces and look for the boundary one
            bool boundaryTriangleFound = false;
            bool contactPointFound = false;
            for (TetrahedronGeometry::FaceIdxIterator face; face.isValid() && !contactPointFound; face++)
            {            
                // is the current face boundary one?
                if (collision->getModelStructure()->getTetrahedralVolumeInterface()->triangleReader()
                    .get(collision->getTetrahedron()->getTriangleID(face))->isBoundary())
                {
                    boundaryTriangleFound = true;                

                    // store the face geometry
                    TriangleGeometry3 tri = tetraGeom.getFaceGeometry(face);
                    Vector3 ip;

                    // compute intersections
                    Math::intersection_t intersection = tri.computeEdgeIntersection(collEdge, ip);

                    // intersection found
                    if (intersection == Math::INTERSECTION_POINT)
                    {
                        // the intersection is our contact point, se correct the position
                        contactPointFound = true;
                        DYLAB_LOGF("\tContact point found: %1%", ip);
						ComModelIntegrator::ScopedExLock(collision->getModelStructure()->getParentSimObject()->getModelIntegrator())
							->correctPosition(ip, collision->getMassPoint());
                    }
                }
            }

            // no contact point found
            if (!contactPointFound)
            {              
                TetrahedronGeometry::pointcls_t pointCls = tetraGeom.classifyPoint(prevPos);

                // previsous point was not inner
                if (pointCls.first != Math::POINTCLS_INNER)
                {
                    // the previous point should lie on the surface of the tested triangle
                    // so we can use the previous point as the contact point                                
                    DYLAB_LOGF2("\tContact not found, using prev pos: %1% (%2%)", prevPos, pointCls);
						ComModelIntegrator::ScopedExLock(collision->getModelStructure()->getParentSimObject()->getModelIntegrator())
							->correctPosition(prevPos, collision->getMassPoint());
                }
                else
                {
                    DYLAB_LOG("\tStrange, the moving point should have been catched early.");
                }
            }

            // no boundary triangle - the collision point is deeper
            if (!boundaryTriangleFound)
            {
                DYLAB_LOG("\tBoundary triangle not found!!!");
                // we have to traverse the hash grid from prev position to collision point
                // and test all boundary faces of the found tetrahedrons again. 
                // Test will end, when we first find the intersection.
            }
        }

        if (mDebugCollisionsEnabled)
        {
            // retrieve our object pool
            ComGfx::IObject * obj 
                = ComGfx::IObjectPool::ScopedExLock(mParentSimulation->getGfx(), OP_PENETRATION_FORCES_NAME)
                    ->newObject();

            // if there is object in the pool available
            if (obj != NULL)
            {
                ComGfx::IObject::PositionBuffer pos;
                pos.insertAtEnd(collPos);
                pos.insertAtEnd(collPos + Vector3(0, 64, 0));
                obj->createSection(mDebugCollisionsMaterial, ComGfx::IObject::ST_LINE_LIST, pos,
                                    ComGfx::IObject::IndexBuffer());
                obj->setRayQueryFlags(0);
            }
        }
    }

    // finally, unlock all processed models
    for (ProcessingModels::Iterator modelInfo(mProcessingModels); modelInfo.isValid(); modelInfo++)    
        modelInfo->getModelStructure()->getParentSimObject()->getModelStructure()->exUnlock();    

    // increace the iteration counter
    mIterationCounter++;
}
//-----------------------------------------------------------------------------